#include <ximera/physics/ode/rigidbody.hpp>
#include <ximera/physics/ode/world.hpp>

namespace ximera { namespace physics { namespace ode
{
    RigidBody::RigidBody(World* world)
        : _flags(0)
    {
        _bodyID = dBodyCreate(world->worldID());
        dBodySetData(_bodyID, this);

        dBodyGetMass(_bodyID, &_mass);
    }

    RigidBody::~RigidBody()
    {
        dBodyDestroy(_bodyID);
    }

    math::Vector3 RigidBody::position() const
    {
        return math::Vector3(dBodyGetPosition(_bodyID));
    }

    void RigidBody::position(math::Vector3 const& pos)
    {
        dBodySetPosition(_bodyID, pos.x, pos.y, pos.z);
    }

    math::Quaternion RigidBody::orientation() const
    {
        return math::Quaternion(dBodyGetQuaternion(_bodyID));
    }

    void RigidBody::orientation(math::Quaternion const& quat)
    {
        dBodySetQuaternion(_bodyID, quat.array());
    }

    math::Matrix3 RigidBody::orientationMatrix() const
    {
        // TODO
        return math::Matrix3();
    }

    void RigidBody::orientationMatrix(math::Matrix3 const& orient)
    {
        // TODO
    }

    math::Vector3 RigidBody::linearVelocity() const
    {
        return math::Vector3(dBodyGetLinearVel(_bodyID));
    }

    void RigidBody::linearVelocity(math::Vector3 const& vel)
    {
        dBodySetLinearVel(_bodyID, vel.x, vel.y, vel.z);
    }

    math::Vector3 RigidBody::angularVelocity() const
    {
        return math::Vector3(dBodyGetAngularVel(_bodyID));
    }

    void RigidBody::angularVelocity(math::Vector3 const& vel)
    {
        dBodySetAngularVel(_bodyID, vel.x, vel.y, vel.z);
    }

    Real RigidBody::mass() const
    {
        return _mass.mass;
    }

    void RigidBody::mass(Real value)
    {
        dMassAdjust(&_mass, value);
        dBodySetMass(_bodyID, &_mass);
    }

    math::Vector3 RigidBody::force() const
    {
        return math::Vector3(dBodyGetForce(_bodyID));
    }

    void RigidBody::force(math::Vector3 const& value)
    {
        dBodySetForce(_bodyID, value.x, value.y, value.z);
    }

    math::Vector3 RigidBody::torque() const
    {
        return math::Vector3(dBodyGetTorque(_bodyID));
    }

    void RigidBody::torque(math::Vector3 const& value)
    {
        dBodySetTorque(_bodyID, value.x, value.y, value.z);
    }

    bool RigidBody::enabled() const
    {
        return dBodyIsEnabled(_bodyID) != 0;
    }

    void RigidBody::enabled(bool value)
    {
        value ? dBodyEnable(_bodyID) : dBodyDisable(_bodyID);
    }

    BodyFlagT RigidBody::flags() const
    {
        return _flags;
    }

    void RigidBody::flags(BodyFlagT value)
    {
        _flags = value;
        dBodySetGravityMode(_bodyID, isFlagSet(BodyFlag::NoGravity));
    }

    bool RigidBody::isFlagSet(BodyFlagT value) const
    {
        return (_flags & value) != 0;
    }

    void RigidBody::setFlag(BodyFlagT value)
    {
        _flags |= value;
        dBodySetGravityMode(_bodyID, !isFlagSet(BodyFlag::NoGravity));
    }

    void RigidBody::unsetFlag(BodyFlagT value)
    {
        _flags &= ~value;
        dBodySetGravityMode(_bodyID, isFlagSet(BodyFlag::NoGravity));
    }    
}}}
